#include "GenericTypeDefs.h"
#include "ISO_ME.h"
#include "Timer.h"
#include "SAEJ1979_table.h"
#include "uart.h"
#define DEBUG_ENABLED

BYTE iso_state=0;
BYTE data_tx[10];
BYTE data_rx[50];
BYTE msg[5];
BYTE msg_len;

BYTE iso_send(BYTE index)
{
    BYTE i=0,j=0;
    BYTE msg[5];
    BYTE ret;
    
    data_tx[0]=0x68;
    data_tx[1]=0x6A;
    data_tx[2]=0xF1;
    
    msg[0]=saej_id_table[index][0];
    msg[1]=saej_id_table[index][1];
    msg_len = saej_id_table[index][3];
    
    for(i=0;i<msg_len;i++)
    {
        data_tx[3+i]=msg[i];
    }
    i=3+i;
    data_tx[i]=checksum(&data_tx[0],i);
    i++;
    
    UART_Print("Send data \n\r");
    for(j=0;j<(i-1);j++)
    {
        UART_Print("%X\t",data_tx[j]);
    }

    ret=iso_put(&data_tx[0],i);
    if(ret != ISO_SEND)
    {
        UART_Print("Error: ISO message sending problems\n\r");
        //iso_state= ISO_UNKNOWN;
        return ISO_BUS_ERROR;
    }
    ret=iso_get(&data_rx[0],10);
    UART_Print("Received data \n\r");
    for(j=0;j<10;j++)
    {
        UART_Print("%X\t",data_rx[j]);
    }
    return ISO_SEND;
}

BYTE check_iso()
{
    BYTE ret;

    ret=iso_5baud();
    if(ret == ISO_DISCONNECT)
    {
        ret=kwp_fast();
    }
    return ret;

}

BYTE iso_5baud()
{
    BYTE temp;
    
    K_HIGH;
    //first make sure that the buss is high for at least W0
    start_timer(20);// W0 20ms
    while(timeout1)
    {
        if (K_IN==0)
            return ISO_DISCONNECT;
    }
    stop_timer();

    UART_Print("5 baud addr=%x\n\r",0x33);
    iso_5baud_putc(0x33);
    data_rx[0]=0;
    temp=iso_get(&data_rx[0],1); //get sync byte
    if ((temp != ISO_RECEIVED)&&(data_rx[0]!=0x55))
    {
        UART_Print("Bad 5baud init %u %X\n\r",temp,data_rx);
        return ISO_DISCONNECT;
    }
    temp=iso_get(&data_rx[0],2);//get key bytes
    if ((temp == ISO_RECEIVED)&&(data_rx[0]==0x08)&&(data_rx[1]==0x08))
    {
        data_tx[0]=~data_rx[0];//
        iso_put(&data_tx[0],1);//Sent ~key byte
        temp=iso_get(&data_rx[0],1);
        if ((temp == ISO_RECEIVED)&&(data_rx[0]==0xCC))
        {
            //iso_state=ISO_9141_5BAUD_CONNECT;
            return ISO_9141_5BAUD_CONNECT;
        }
        else
        {
            UART_Print("Bad keys\n\r");
            UART_Print("Error: ISO 5 baud failed %X %X\n\r",data_rx[0],temp);
            return ISO_DISCONNECT;

        }
    }
    else if((temp == ISO_RECEIVED)&&(data_rx[0]==0x8F)&&
            ((data_rx[1]==0xE9)||(data_rx[1]==0x6B)||(data_rx[1]==0x6D)||(data_rx[1]==0xEF)))
    {
        data_tx[0]=~data_rx[1];//
        iso_put(&data_tx[0],1);//Sent ~key byte
        temp=iso_get(&data_rx[0],1);
        if ((temp == ISO_RECEIVED)&&(data_rx[0]==0xCC))
        {
            //iso_state=ISO_9141_5BAUD_CONNECT;
            return KWP_SLOW_CONNECT;
        }
        else
        {
            UART_Print("Bad keys\n\r");
            UART_Print("Error: ISO 5 baud failed %X %X\n\r",data_rx[0],temp);
            return ISO_DISCONNECT;
        }
    }
    else
    {
     UART_Print("Bad keys\n\r");
     UART_Print("5bps Keys %x %x, resp %x\n\r",data_rx[0],data_rx[1],temp);
     return ISO_DISCONNECT;
    }
}


BYTE kwp_fast()
{
    BYTE temp,i=0,j=0,ret=0;

    K_HIGH;
    //first make sure that the buss is high for at least T idle
    start_timer(300);//T idle 300ms after power on,55ms after stop comm, 0ms after timeout P3
    while(timeout1)
    {
        if (K_IN==0)
            return ISO_DISCONNECT;
    }
    stop_timer();

    K_LOW;
    start_timer(24);//T iniL 24ms
    while(timeout1)
    {
        if (K_IN==1)
            return ISO_DISCONNECT;
    }
    stop_timer();

    K_HIGH;
    start_timer(49);//T WuP 300ms after power on,55ms after stop comm, 0ms after timeout P3
    while(timeout1)
    {
        if (K_IN==0)
            return ISO_DISCONNECT;
    }
    stop_timer();

    data_tx[0]=0xC1;
    data_tx[1]=0x33;
    data_tx[2]=0xF1;
    data_tx[3]=0x81;
    data_tx[4]=0x66;
    //data_tx[4]=checksum(&data_tx[0],3);

    UART_Print("Send data \n\r");
    for(j=0;j<5;j++)
    {
        UART_Print("%X\t",data_tx[j]);
    }
    ret=iso_put(&data_tx[0],5);
    if(ret != ISO_SEND)
    {
        UART_Print("Error: ISO message sending problems\n\r");
        return ISO_BUS_ERROR;
    }
    temp=iso_get(&data_rx[0],7); //get sync bytes
    if ((temp != ISO_RECEIVED)&&(data_rx[0]!=0x83)&&(data_rx[1]!=0xF1)
            &&(data_rx[2]!=0x10)&&(data_rx[3]!=0xC1))
    {
        UART_Print("Bad 5baud init %u %X\n\r",temp,data_rx);
        return ISO_DISCONNECT;
    }
    else
    {
        return KWP_FAST_CONNECT;
    }   
}

//sends out a char at 5-baud rate
void iso_5baud_putc(BYTE data)
{
    BYTE i;

    //first drive low for start bit
    K_LOW;
    L_LOW;
    delayMs(200);

    for(i=0; i<8; i++)
    {
        if(data & 0x01)
        {
            K_HIGH;
            L_HIGH;
        } else
        {
            K_LOW;
            L_LOW;
        }
        data=data>>1;
        delayMs(200);
    }
    //output stop bit
    K_HIGH;
    L_HIGH;
    delayMs(200);
}

BYTE iso_put(BYTE *data, BYTE len)
{
    BYTE mask;
    BYTE temp;
    BYTE i=0;

    K_HIGH;//make sure buss is high

    start_timer(50);
    while(!timeout1)	//spin bus to go high
    {
        if (!K_IN)
        {
            UART_Print("Error: Put failed\n\r");
            return ISO_BUS_ERROR;
        }
    }
    stop_timer();
    
    while(len != 0)
    {
        temp=*data;
        mask=0x01;
        //drive with start bit, then send data
        K_LOW;
        delayMs(50); //wait a bit
        for (i=0;i<8;i++)
        {
            if (temp & mask)
            {
                K_HIGH;
            }else
            {
                K_LOW;
            }
            mask=mask<<1;
            start_timer(50);// W0 20ms
            while(timeout1); //while(TMR0<baud);
            stop_timer();
        }
        //now we need to send stop bit
        K_HIGH;
        start_timer(50);// W0 20ms
        while(timeout1); //while(TMR0<baud);
        stop_timer();

        //now wait to send next byte
        start_timer(ISO_P4_MIN);
        while(!timeout1)	//spin bus to go high
        {
            if (!K_IN)
            {
                UART_Print("ERROR: ISO put failed\n\r");
                return ISO_BUS_ERROR;
            }
        }
        stop_timer();

        len--;
    }
    return ISO_SEND;
}

//Well I need an Async get char function
BYTE iso_get(BYTE *data, BYTE len)
{
    BYTE temp=0;
    BYTE i=0;

    while(len != 0)
    {
        K_HIGH;	   //make sure bus is high...
        while(K_IN==0)	//spin bus to go high
        {
            start_timer(50);
            if(timeout1)
            {
                UART_Print("Error: ISO get bus error\n\r");
                return ISO_BUS_ERROR;
            }
        }
        stop_timer();
        while(K_IN)	//spin waiting on sync bit
        {
            start_timer(50);// W0 20ms
            if(timeout1)
            {
                UART_Print("Error: ISO get time out\n\r");
                return ISO_TIMEOUT;
            }
        }
        stop_timer();

        //start bit just went low
        //wait a bit and half and read data
        delayMs(200); //wait a bit and half and read data
        temp=0;
        for(i=0; i<8; i++)
        {
            temp=temp>>1;
            if (K_IN)
            {
                    temp=temp | 0x80;
            }
            start_timer(50);// W0 20ms
            while(timeout1); //while(TMR0<baud);
            stop_timer();
        }
        *data=temp;
        data++;
        len--;
    }
    return ISO_RECEIVED;
}

BYTE checksum(BYTE *data, BYTE len)
{
    BYTE i;
    BYTE sum;

    sum=0;
    for(i=0; i<len; i++)
    {
            sum=sum+(*data);
            data++;
    }
    return sum;
}
